/**
 * @file TopCapObsPlanner.h
 * @author  
 * @brief 上盖障碍物异常控制设定
 * @version 1.0
 * @date 2020-07-23
 * 
 * @copyright Copyright (c) 2020
 * 
 */

#pragma once
#include "lib/contour.h"
#include "data_struct/geometry/PointCloud.h"

namespace behavior_controller
{
    #define NONEXIST_OBS 0
    #define STATIC_OBS 1
    #define DYNAMIC_OBS 2
    class TopCapObsPlanner
    {
    public:
        /**
         * @brief 上盖障碍物响应规划的构造函数
         * @return
         */
        TopCapObsPlanner(){};

        /**
         * @brief 上盖障碍物响应的运行函数
         * @return
         */
        void run(double &linear_vel, double &angular_vel);

        /**
         * @brief 控制机器人后退函数，退出障碍物区域
         * @return
         */
        void Avoid(double &linear_vel, double &angular_vel);

        /**
         * @brief 控制机器人转向函数，转到平行障碍物区域
         * @return
         */
        void TurnToAngle(double &linear_vel, double &angular_vel,double &dst_angle);

        /**
         * @brief 实时检查上盖区域的障碍物
         * @return
         */
        int CheakTopCapObs(PointCloud<double> &in_contour_points);
        Contour robot_shape_new;
        double last_angle;     //激光点数据大于5个时记录下来的机器人朝向，用于转向的起始角度
        double angle_diff;     //检测到悬崖直线后，机器人与直线的角度差
        double global_goal_angle;
        double meanx,meany;
        double time=0;
        double avoid_speed;
        int obs_type=NONEXIST_OBS;
        bool avoid_finished=true;
        bool finished = true; //沿悬崖动作是否结束的标记
        bool init_angle=false;
        bool finished_turn=false;
        bool init_pose=false;
        
    };

    //static 
    extern TopCapObsPlanner top_cap_obs_planner;
} // namespace behavior_controller